import rospy
from pyzbar import pyzbar
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from clover import srv
from std_srvs.srv import Trigger

bridge = CvBridge()

rospy.init_node('barcode_test')

get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)
set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
land = rospy.ServiceProxy('land', Trigger)

navigate(x=0, y=0, z=1, frame_id='body', auto_arm=True)

rospy.sleep(3)

navigate(x=0, y=0.5, z=0.7, frame_id='aruco_map')

# Image subscriber callback function
def image_callback(data):
    cv_image = bridge.imgmsg_to_cv2(data, 'bgr8')  # OpenCV image
    barcodes = pyzbar.decode(cv_image)
    for barcode in barcodes:
        b_data = barcode.data.encode("utf-8")
        b_type = barcode.type
        (x, y, w, h) = barcode.rect
        xc = x + w/2
        yc = y + h/2
        print ("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))

image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1)


navigate(x=0, y=1.5, z=1.5, frame_id='aruco_map')

rospy.sleep(8)

navigate(x=0.5, y=2.7, z=1.5, frame_id='aruco_map')

rospy.sleep(8)

navigate(x=1.25, y=1, z=1.5, frame_id='aruco_map')

rospy.sleep(8)

navigate(x=2.2, y=0.5, z=1.5, frame_id='aruco_map')

rospy.sleep(8)

land()